/*
 * joey_routines.c
 *
 *  Created on: Aug 19, 2009
 *      Author: Robotics
 */


#include "ifi_aliases.h"
#include "ifi_default.h"
#include "joey_routines.h"

//TODO: Expand this description. This function does more than that.

unsigned int abst(signed int number) {
	if (number < 0){
		return number*-1;
	}

	if (number >= 0){
		return number;
	}
}

signed int getJoystickValue(unsigned char in){
	signed int x = 0;
	x = x + in;

	return x - 127;
}

//Finds the maximum drive value. range -127 - 127
void setDriveValues(signed int fl, signed int fr, signed int bl, signed int br)
{
	unsigned int max = 0;

	if (abst(fl) > max)
	{
		max = abst(fl);
	}

	if (abst(fr) > max)
	{
		max = abst(fr);
	}

	if (abst(bl) > max)
	{
		max = abst(bl);
	}

	if (abst(br) > max)
	{
		max = abst(br);
	}

//Scales down the drive values if the maximum drive value is greater than 127.
	if (max > 127)
	{
		float wheelLimit = 127.0 / (float)max;
		fl = (signed int)((float)fl * wheelLimit);
		fr = (signed int)((float)fr * wheelLimit);
		bl = (signed int)((float)bl * wheelLimit);
		br = (signed int)((float)br * wheelLimit);
	}

//Reverse the right motors.
	fr = -fr;
	br = -br;

//Converts all motor values to range 0 to 255.
	FRONT_LEFT_WHEEL_MOTOR = (char)(fl + 127);
	FRONT_RIGHT_WHEEL_MOTOR = (char)(fr + 127);
	BACK_LEFT_WHEEL_MOTOR = (char)(bl + 127);
	BACK_RIGHT_WHEEL_MOTOR = (char)(br + 127);

}


char controlValueLeftY, controlValueLeftX, controlValueRightX;
char virtControlLeftY, virtControlLeftX, virtControlRightX;

void driveRobot(char autonomousMode)
{
	//Create an array for drive values.
	int driveArray[4];

	//Change controller variables depending on autonomous or not
	if (autonomousMode)
	{
		controlValueLeftY = virtControlLeftY;
		controlValueLeftX = virtControlLeftX;
		controlValueRightX = virtControlRightX;
	}

	else
	{
		controlValueLeftY = CONTROL_LEFT_Y;
		controlValueLeftX = CONTROL_LEFT_X;
		controlValueRightX = CONTROL_RIGHT_X;
	}

	//Assign values in array to joystick values in order of fl, fr, bl, br wheels.
	driveArray[0] = (getJoystickValue(controlValueLeftY) + getJoystickValue(controlValueLeftX) - getJoystickValue(controlValueRightX));
	driveArray[1] = (getJoystickValue(controlValueLeftY) + getJoystickValue(controlValueLeftX) + getJoystickValue(controlValueRightX));
	driveArray[2] = (-getJoystickValue(controlValueLeftY) - getJoystickValue(controlValueLeftX) - getJoystickValue(controlValueRightX));
	driveArray[3] = (-getJoystickValue(controlValueLeftY) - getJoystickValue(controlValueLeftX) + getJoystickValue(controlValueRightX));

	//Send drive values to motor.
	setDriveValuesP(&driveArray);

}

signed char setDriveValuesP(int* driveValues)
{
	unsigned int max = 0;

	int i=0;
	//Find biggest number to use as a ratio.
	for (i=0; i<4; i++)
	{
		if (abst(driveValues[i]) > max)
		{
			max = abst(driveValues[i]);
		}
	}

	//Scales down the drive values if the maximum drive value is greater than 127.
	if (max > 127)
	{
		float wheelLimit = 127.0 / (float)max;

		int x=0;
		for (x=0; x<4; x++)
		{
			driveValues[x] = (signed int)((float)driveValues[x] * wheelLimit);
		}
	}

	//Reverse the right motors.
	driveValues[1] = -driveValues[1];
	driveValues[3] = -driveValues[3];

	//Converts all motor values to range 0 to 255.

	FRONT_LEFT_WHEEL_MOTOR = (char)(driveValues[0] + 127);
	FRONT_RIGHT_WHEEL_MOTOR = (char)(driveValues[1] + 127);
	BACK_LEFT_WHEEL_MOTOR = (char)(driveValues[2] + 127);
	BACK_RIGHT_WHEEL_MOTOR = (char)(driveValues[3] + 127);
}



//Converts all of the motor values for the claw and descore to range 0 to 255.
/*void turnClaw(signed int control){

	TURN_CLAW_MOTOR = (char)(control + 127);
*/

/*	Joey, There were a set of library files that needed to be updated to
*	go with the VEXnet upgrade.
*	The files are exactly what they sound like.  Libraries of functions written
*	by the people at VEX.
*	They are the files with the .lib on the end.
*	I updated them.  Hopefully that fixes the problem of the Vex controller not working!!
*	Also can you relay to Jannelle or Marygold that our team should be registered at:
*			http://www.vexforum.com/wiki/index.php/TeamList
*	They can collect the needed information and register our team.
*/

/*
unsigned char limitToChar(signed int motorValue){
	unsigned char output = 127;
	if(motorValue > (signed int)255){
		output = 255;
	}

	else if(motorValue < (signed int)0){
		//TODO: error in comparison
		output = 0;
	}
	else {
		//TODO:Problem here with typecasting
		output = (unsigned char)motorValue;
	}
	return output;
} */


void liftFork(char control){
	LEFT_LIFT_FORK_MOTOR = control;
	RIGHT_LIFT_FORK_MOTOR = 255  - control ;

}

void liftHopper(char control){
	LEFT_LIFT_HOPPER_MOTOR = control ;
	RIGHT_LIFT_HOPPER_MOTOR = control ;

}

